#!/usr/bin/env python
#coding=utf-8

import rospy
from geometry_msgs.msg import Point, PoseStamped
from ar_track_alvar_msgs.msg import AlvarMarkers
from xarm_vision.srv import *
class TagsPose():
    def __init__(self):
        rospy.init_node("ar_tags_pose")

        # 话题ar_pose_marker的订阅端，回调函数为get_tags
        rospy.Subscriber("ar_pose_marker", AlvarMarkers, self.get_tags)

        # 定义get_tag_pose服务，用来获取某ID标签的位姿
        self.pick_place_srv = rospy.Service('/get_tag_pose', GetTagPose, self.call_tag_pose)

        # 初始化tag_result为AlvarMarkers消息类型
        self.tag_result = AlvarMarkers()

        rospy.loginfo("starting ...")

    # get_tag_pose服务的处理函数
    def call_tag_pose(self, req):
        tag_pose = PoseStamped()

        n = len(self.tag_result.markers)
        # 如果没有检测到标签，服务响应的success为False
        if n == 0:
            return GetTagPoseResponse(False,tag_pose)

        # 在检测到的标签数组里判断是否存在服务想要获取的ID标签，若存在，响应为True且返回此标签的位姿
        for tag in self.tag_result.markers:
            if req.id != tag.id :
                continue
            tag_pose = tag.pose
            tag_pose.header.stamp = rospy.Time.now()
            tag_pose.header.frame_id = self.tag_result.markers[0].header.frame_id
            return GetTagPoseResponse(True,tag_pose)
        return GetTagPoseResponse(False,tag_pose)

    # ar_pose_marker话题回调函数
    def get_tags(self, msg):
        self.tag_result = msg


if __name__ == '__main__':
    try:
        TagsPose()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("AR Tag Tracker node terminated.")
